import os
import sys
import numpy as np

np.set_printoptions(suppress=True)

def read_gps_data(path):

    gps_data =[]
    gps_start_timestamp = 0

    with open(path) as f:
        for line in f:
            if line[0] == '#' or line == "":
                continue
            line1 = line.rstrip()
            mm1 = line1.split(' ')
            pts = (mm1[0].split(':'))[1]
            pts = int(pts)

            fix_type = (mm1[3].split(':'))[1]
            if int(fix_type) < 3:
                continue

            timestamp = (mm1[4].split(':'))[1]
            timestamp = int(timestamp)
            if pts == 0:
                gps_start_timestamp = timestamp

            latitude = (mm1[5].split(':'))[1]
            latitude = float(latitude)
            longitude = (mm1[6].split(':'))[1]
            longitude = float(longitude)
            altitude = (mm1[7].split(':'))[1]
            altitude = float(altitude)

            gps_data.append([pts, timestamp, latitude, longitude, altitude])

    gps_data = np.array(gps_data)
    return [gps_data, gps_start_timestamp]


def save_gps_data(data, path):

    input_gps = open(path, "w+")

    for i in range(data.shape[0]):
        input_gps.write("{:.0f}".format(data[i,1]*1e3))
        input_gps.write(" ")
        input_gps.write(str(data[i,2]))
        input_gps.write(" ")
        input_gps.write(str(data[i,3]))
        input_gps.write(" ")
        input_gps.write(str(data[i,4]))
        input_gps.write("\n")

    input_gps.close()


def read_imu_data(path):

    imu_data = []

    g = 9.80665

    with open(path) as f:
        for line in f:
            if line[0] == '#' or line == "":
                continue
            line1 = line.rstrip()
            mm1 = line1.split(' ')
            pts = (mm1[0].split('='))[1]
            pts = int(pts)
            gyro_x = float(mm1[3])
            gyro_y = float(mm1[4])
            gyro_z = float(mm1[5])
            accel_x = float(mm1[7]) * g
            accel_y = float(mm1[8]) * g
            accel_z = float(mm1[9]) * g

            imu_data.append([pts, gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z])

    imu_data = np.array(imu_data)
    return imu_data


def save_imu_data(start_timestamp, data, path):

    input_imu = open(path, "w+")
    input_imu.write("#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]\n")

    for i in range(data.shape[0]):
        input_imu.write("{:.0f}".format((start_timestamp + data[i,0])*1e3))
        input_imu.write(",")
        input_imu.write(str(data[i,1]))
        input_imu.write(",")
        input_imu.write(str(data[i,2]))
        input_imu.write(",")
        input_imu.write(str(data[i,3]))
        input_imu.write(",")
        input_imu.write(str(data[i,4]))
        input_imu.write(",")
        input_imu.write(str(data[i,5]))
        input_imu.write(",")
        input_imu.write(str(data[i,6]))
        input_imu.write("\n")

    input_imu.close()


def read_timestamp_data(start_timestamp, path):

    timestamp_data = []

    f = open(path)
    line = f.readline()
    while line != "":
        line1 = line.rstrip()
        mm1 = line1.split(' ')
        pts = (mm1[0].split(':'))[1]
        pts = int(pts)
        timestamp_data.append(start_timestamp + pts)

        line = f.readline()
        line = f.readline()
        line = f.readline()
        line = f.readline()
        line = f.readline()
        line = f.readline()

    return timestamp_data


def save_image_timestamp(data, path):

    input_image_timestamp = open(path, "w+")

    for i in range(len(data)):
        input_image_timestamp.write("{:.0f}".format(data[i]*1e3))
        input_image_timestamp.write("\n")

    input_image_timestamp.close()

def main():
    # dir_path = "/media/cjg/Elements/pano20210730/1/VID_20210730_102531/sensor_data/"
    # image_start_timestamp = 1627611933805260

    dir_path = "/media/cjg/Elements/pano20210902/pro2/data"
    image_start_timestamp = 1630485593424254

    [gps_data, gps_start_timestamp] = read_gps_data(dir_path + "/gps.txt")
    imu_data = read_imu_data(dir_path + "/gyro.txt")
    image_timestamp = read_timestamp_data(image_start_timestamp, dir_path + "/exposure.txt")

    save_gps_data(gps_data, dir_path + "/gps_new.txt")
    save_imu_data(gps_start_timestamp, imu_data, dir_path + "/imu_new.txt")
    # save_imu_data(image_start_timestamp, imu_data, dir_path + "/imu_new.txt")
    save_image_timestamp(image_timestamp, dir_path + "/image_timestamp.txt")

if __name__ == '__main__':
    main()
